{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### B样条曲线法实现车辆轨迹规划"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 164,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import copy\n",
    "from celluloid import Camera  # 保存动图时用，pip install celluloid\n",
    "%matplotlib qt5\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 基函数实现\n",
    "\n",
    "![在这里插入图片描述](https://img-blog.csdnimg.cn/ea7173523b57448dad3d7a29393a36b7.png)\n",
    "\n",
    "\n",
    "如果遇到分母为 0的情况：如果此时分子也为0，约定这一项整体为0；如果此时分子不为0，则约定分母为1 。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 165,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "def BaseFunction(i=None, k=None, u=None, NodeVector=None):\n",
    "    \"\"\"第 i个k阶B样条基函数\n",
    "\n",
    "    Args:\n",
    "        i (_type_, optional): _description_. Defaults to None.\n",
    "        k (_type_, optional): B样条阶数k. Defaults to None.\n",
    "        u (_type_, optional): 自变量. Defaults to None.\n",
    "        NodeVector (_type_, optional): 节点向量. array([u0,u1,u2,...,u_n+k],shape=[1,n+k+1].\n",
    "\n",
    "    Returns:\n",
    "        _type_: _description_\n",
    "    \"\"\"\n",
    "    if k == 1: # 0次B样条（1阶B样条）\n",
    "        if u >= NodeVector[0,i] and u < NodeVector[0,i + 1]:\n",
    "            Bik_u = 1\n",
    "        else:\n",
    "            Bik_u = 0\n",
    "    else:\n",
    "        # 公式中的两个分母\n",
    "        denominator_1 = NodeVector[0,i + k - 1] - NodeVector[0,i]\n",
    "        denominator_2 = NodeVector[0,i + k] - NodeVector[0,i + 1]\n",
    "        # 如果遇到分母为 0的情况：\n",
    "        # 1. 如果此时分子也为0，约定这一项整体为0；\n",
    "        # 2. 如果此时分子不为0，则约定分母为1 。\n",
    "        if denominator_1 == 0: \n",
    "            denominator_1 = 1\n",
    "        if denominator_2 == 0:\n",
    "            denominator_2 = 1\n",
    "        Bik_u = (u - NodeVector[0,i ]) / denominator_1 * BaseFunction(i, k - 1, u, NodeVector) + \\\n",
    "            (NodeVector[0,i + k] - u) / denominator_2 * \\\n",
    "            BaseFunction(i + 1, k - 1, u, NodeVector)\n",
    "\n",
    "    return Bik_u\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 准均匀B样条的节点向量计算\n",
    "\n",
    "特点是两端节点具有重复度k,一般取值范围为[0,1]\n",
    "\n",
    "共n+1个控制顶点，k-1次B样条，k阶\n",
    "\n",
    "\n",
    "![在这里插入图片描述](https://img-blog.csdnimg.cn/d56a3356949a4a7da6e28860908c43bb.png)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 166,
   "metadata": {},
   "outputs": [],
   "source": [
    "def U_quasi_uniform(n = None,k = None): \n",
    "    \"\"\"准均匀B样条的节点向量计算\n",
    "    首末值定义为 0 和 1\n",
    "    Args:\n",
    "        n (_type_, optional): 控制点个数-1，控制点共n+1个. Defaults to None.\n",
    "        k (_type_, optional): B样条阶数k， k阶B样条，k-1次曲线. Defaults to None.\n",
    "\n",
    "    Returns:\n",
    "        _type_: _description_\n",
    "    \"\"\"\n",
    "    # 准均匀B样条的节点向量计算，共n+1个控制顶点，k-1次B样条，k阶\n",
    "    NodeVector = np.zeros((1,n + k + 1))\n",
    "    piecewise = n - k + 2  # B样条曲线的段数:控制点个数-次数\n",
    "    \n",
    "    if piecewise == 1:  # 只有一段曲线时，n = k-1\n",
    "        NodeVector[0,n+1:n+k+1] = 1\n",
    "    else:\n",
    "        for i in range(n-k+1):  # 中间段内节点均匀分布：两端共2k个节点，中间还剩(n+k+1-2k=n-k+1）个节点\n",
    "            NodeVector[0, k+i] = NodeVector[0, k+i-1]+1/piecewise\n",
    "\n",
    "        NodeVector[0,n + 1:n + k + 1] = 1  # 末尾重复度k\n",
    "    \n",
    "    return NodeVector"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 167,
   "metadata": {},
   "outputs": [],
   "source": [
    "def U_piecewise_B_Spline(n = None,k = None): \n",
    "    \"\"\"分段B样条的节点向量计算\n",
    "    首末值定义为 0 和 1\n",
    "    # 分段Bezier曲线的节点向量计算，共n+1个控制顶点，k阶B样条，k-1次曲线\n",
    "    # 分段Bezier端节点重复度为k，内间节点重复度为k-1,且满足n/(k-1)为正整数\n",
    "    Args:\n",
    "        n (_type_, optional): 控制点个数-1，控制点共n+1个. Defaults to None.\n",
    "        k (_type_, optional): B样条阶数k， k阶B样条，k-1次曲线. Defaults to None.\n",
    "\n",
    "    Returns:\n",
    "        _type_: _description_\n",
    "    \"\"\"\n",
    "\n",
    "    \n",
    "    NodeVector = np.zeros((1,n + k + 1)) \n",
    "    if n%(k-1)==0 and k-1 > 0:  # 满足n是k-1的整数倍且k-1为正整数\n",
    "        NodeVector[0,n + 1:] = 1 # 末尾n+1到n+k+1的数重复\n",
    "        piecewise = n / (k-1)  # 设定内节点的值\n",
    "        if piecewise > 1:\n",
    "            for i in range(1,int(piecewise)):\n",
    "                # for j in range(0,k-1):# 内节点重复度k-1\n",
    "                #     NodeVector[0, (k-1)*i+1+j] = i / piecewise  \n",
    "                NodeVector[0, (k-1)*i+1:(k-1)*i+k] = i / piecewise  # 内节点重复度k-1\n",
    "    else:\n",
    "        print('error!需要满足n是k-1的整数倍且k-1为正整数')\n",
    "    print(\"node:\",NodeVector)\n",
    "    \n",
    "    return NodeVector"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 参数设置"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 168,
   "metadata": {},
   "outputs": [],
   "source": [
    "## 数据定义\n",
    "k = 3  # k阶、k-1次B样条\n",
    "\n",
    "flag = 3  # 1,2,3分别绘制均匀B样条曲线、准均匀B样条曲线,分段B样条\n",
    "\n",
    "d = 3.5  # # 道路标准宽度\n",
    "# 控制点\n",
    "P = np.array([\n",
    "    [0, -d / 2],\n",
    "    [10, -d / 2],\n",
    "    [25, -d / 2 + 0.5],\n",
    "    [25, d / 2 - 0.5],\n",
    "    [40, d / 2],\n",
    "    [50, d / 2],\n",
    "    [60, d / 2]\n",
    "    ])\n",
    "\n",
    "n = len(P)-1 # 控制点个数-1\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 生成B样条曲线"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 169,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "node: [[0.         0.         0.         0.33333333 0.33333333 0.66666667\n",
      "  0.66666667 1.         1.         1.        ]]\n"
     ]
    }
   ],
   "source": [
    "## 生成B样条曲线\n",
    "\n",
    "path = [] # 路径点数据存储\n",
    "Bik_u = np.zeros((n+1, 1))\n",
    "\n",
    "if flag == 1:  # 均匀B样条很简单\n",
    "    NodeVector = np.array([np.linspace(0, 1, n + k + 1)])  # 均匀B样条节点向量，首末值定义为 0 和 1\n",
    "    # for u in np.arange(0,1,0.001):\n",
    "    for u in np.arange((k-1)  / (n + k + 1), (n + 2) / (n + k + 1)+0.001, 0.001): # u的范围为[u_{k-1},u_{n+2}],这样才是open的曲线，不然你可以使用[0,1]试试。\n",
    "        for i in range(n+1):\n",
    "            Bik_u[i, 0] = BaseFunction(i, k, u, NodeVector)\n",
    "        p_u = P.T @ Bik_u\n",
    "        path.append(p_u)\n",
    "elif flag == 2:\n",
    "    NodeVector = U_quasi_uniform(n, k)\n",
    "    for u in np.arange(0, 1, 0.005):\n",
    "        for i in range(n+1):\n",
    "            Bik_u[i, 0] = BaseFunction(i, k, u, NodeVector)\n",
    "        p_u = P.T @ Bik_u\n",
    "        path.append(p_u)\n",
    "elif flag==3:\n",
    "    NodeVector = U_piecewise_B_Spline(n, k)\n",
    "    for u in np.arange(0, 1, 0.005):\n",
    "        for i in range(n+1):\n",
    "            Bik_u[i, 0] = BaseFunction(i, k, u, NodeVector)\n",
    "        p_u = P.T @ Bik_u\n",
    "        path.append(p_u)\n",
    "path=np.array(path)\n",
    "\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 画图"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 170,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "\n",
    "\n",
    "## 画图\n",
    "fig = plt.figure(1)\n",
    "# plt.ylim(-4, 4)\n",
    "# plt.axis([-10, 100, -15, 15])\n",
    "camera = Camera(fig)\n",
    "len_line = 50\n",
    "# 画灰色路面图\n",
    "GreyZone = np.array([[- 5, - d - 0.5], [- 5, d + 0.5],\n",
    "                     [len_line, d + 0.5], [len_line, - d - 0.5]])\n",
    "for i in range(len(path)):\n",
    "    # plt.cla()\n",
    "\n",
    "    plt.fill(GreyZone[:, 0], GreyZone[:, 1], 'gray')\n",
    "    # 画分界线\n",
    "    plt.plot(np.array([- 5, len_line]), np.array([0, 0]), 'w--')\n",
    "\n",
    "    plt.plot(np.array([- 5, len_line]), np.array([d, d]), 'w')\n",
    "\n",
    "    plt.plot(np.array([- 5, len_line]), np.array([- d, - d]), 'w')\n",
    "\n",
    "    plt.plot(P[:,0],P[:,1],'ro')\n",
    "    plt.plot(P[:, 0], P[:, 1], 'y')\n",
    "    # 设置坐标轴显示范围\n",
    "    # plt.axis('equal')\n",
    "    plt.gca().set_aspect('equal')\n",
    "    # 绘制路径\n",
    "\n",
    "    plt.plot(path[0:i, 0], path[0:i, 1], 'g')  # 路径点\n",
    "    # plt.pause(0.001)\n",
    "    camera.snap()\n",
    "animation = camera.animate()\n",
    "if flag==1:\n",
    "    animation.save('均匀.gif')\n",
    "elif flag==2:\n",
    "    animation.save('准均匀.gif')\n",
    "else:\n",
    "    animation.save('分段.gif')\n"
   ]
  }
 ],
 "metadata": {
  "interpreter": {
   "hash": "0c7484b3574347463e16b31029466871583b0d4e5c4ad861e8848f2d3746b4de"
  },
  "kernelspec": {
   "display_name": "Python 3.8.12 ('gobigger')",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.12"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
